#include "robot_cmd.h"
#include "board_com.h"
#include "drivers/dev_pin.h"
#include "drv_flash.h"
#include "message.h"
#include "offline.h"
#include "robotdef.h"
#include "rtklibc.h"
#include "rtthread.h"
#include "rttypes.h"
#include "sbus.h"


#define LOG_TAG              "robotcmd"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


extern sbus_s sbus;
// static struct Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
// static struct Vision_Send_s vision_send_data;  // 视觉发送数据

static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息
static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
static Subscriber_t *shoot_feed_sub;         // 发射反馈信息订阅者
static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等

void RobotCMDInit(void)
{



#ifdef BOARD_BIG_YAW
    gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

    board_com.board_tx.can_handle=&can1_bus;
    board_com.board_tx.txmsg.id=BOARD_BIG_YAW_ID;
    board_com.board_tx.txmsg.ide =RT_CAN_STDID;
    board_com.board_tx.txmsg.rtr =RT_CAN_DTR;
    board_com.board_tx.txmsg.len =8;
    board_com.send_data_len = sizeof(chassis_cmd_send);
    board_com.recv_data_len = sizeof(chassis_fetch_data);
    board_com.recv_buf_len = board_com.recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
    board_com.send_buf_len = board_com.send_data_len + CAN_COMM_OFFSET_BYTES;
    board_com.raw_sendbuf[1] = board_com.send_data_len; // datalen
    board_com.raw_sendbuf[board_com.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
#endif 
}

/**
 * @brief 根据gimbal app传回的当前电机角度计算和零位的误差
 *        单圈绝对角度的范围是0~360,说明文档中有图示
 *
 */
static void CalcOffsetAngle()
{
    // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
    static float angle;
    angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
#if YAW_ECD_GREATER_THAN_4096                               // 如果大于180度
    if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
    else if (angle > 180.0f + YAW_ALIGN_ANGLE)
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
    else
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
#else // 小于180度
    if (angle > YAW_ALIGN_ANGLE)
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
    else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f)
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
    else
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
#endif
}

/**
 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
 *
 */
static void RemoteControlSet()
{

    if (offline_manage[OFFLINE_SBUS].online_state == STATE_ONLINE)
    {
        // 云台参数,确定云台控制数据
        // 按照摇杆的输出大小进行角度增量,增益系数需调整
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
        gimbal_cmd_send.yaw         -= 0.005f * (float)(sbus.SBUS_CH.CH4 - SBUS_CHX_BIAS);
        gimbal_cmd_send.pitch       -= 0.001f * (float)(sbus.SBUS_CH.CH3 - SBUS_CHX_BIAS);

        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
        chassis_cmd_send.vx  = 50.0f * (float)(sbus.SBUS_CH.CH2 - SBUS_CHX_BIAS);
        chassis_cmd_send.vy  = -50.0f * (float)(sbus.SBUS_CH.CH1 - SBUS_CHX_BIAS);

    }
    else {
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
        chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
    }

    if (sbus.SBUS_CH.CH5 == 240)
    {
        shoot_cmd_send.shoot_mode = SHOOT_OFF;
        shoot_cmd_send.friction_mode = FRICTION_OFF;
        shoot_cmd_send.shoot_rate = LOAD_STOP;
    }
    else if (sbus.SBUS_CH.CH5 == 1024) {
        shoot_cmd_send.shoot_mode = SHOOT_ON;
    }
    else if (sbus.SBUS_CH.CH5 == 1807) {
        shoot_cmd_send.friction_mode = FRICTION_ON;
        if (sbus.SBUS_CH.CH7 == SBUS_CHX_BIAS)
        {
            shoot_cmd_send.load_mode = LOAD_STOP;
            shoot_cmd_send.shoot_rate = 0;
        }
        else if (sbus.SBUS_CH.CH7 == 1807) {
            shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
            shoot_cmd_send.shoot_rate = 30;
        }
        else if (sbus.SBUS_CH.CH7 == 240) {
            shoot_cmd_send.load_mode = LOAD_1_BULLET;
            shoot_cmd_send.shoot_rate = 1;
        }
        else { shoot_cmd_send.load_mode = LOAD_STOP; }
    } 

    if (offline_manage[OFFLINE_SBUS].online_state == STATE_ONLINE && sbus.SBUS_CH.CH8==1024) {
        chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
    }
    else if (offline_manage[OFFLINE_SBUS].online_state == STATE_ONLINE && sbus.SBUS_CH.CH8==1807) 
    {
        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
        chassis_cmd_send.wz  = 20.0f *  (float)(sbus.SBUS_CH.CH9 - 240);
        chassis_cmd_send.vx  = 50.0f * (float)(sbus.SBUS_CH.CH2 - SBUS_CHX_BIAS);
        chassis_cmd_send.vy  = -50.0f * (float)(sbus.SBUS_CH.CH1 - SBUS_CHX_BIAS);
    }
}


/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
static void robotcmdtask(void *paramter)
{
    while (1) {
#ifdef BOARD_SMALL_YAW_1 
        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
#endif
#ifdef BOARD_SMALL_YAW_2
        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
#endif
#ifdef BOARD_BIG_YAW
        SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
        RemoteControlSet();
        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        CalcOffsetAngle();
        //LOG_D("chassis yaw %f\n",chassis_cmd_send.offset_angle);
        static rt_uint8_t buffer[sizeof(chassis_cmd_send)]={0};
        rt_memcpy(buffer,&chassis_cmd_send,sizeof(chassis_cmd_send));
        board_send(buffer);
#endif
        rt_thread_mdelay(5);
    }
}

void BMI088_ERASE_Callback()
{
    rt_uint8_t tmpdata[21] = {0};
    stm32_flash_erase((uint32_t)0x080E1000, sizeof(tmpdata));
}


int robotcmd_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("robotcmd", robotcmdtask, RT_NULL, 1024, 30, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("robotcmd thread init success!\n");
    
    RobotCMDInit();
    rt_pin_mode(GET_PIN(A,0),PIN_MODE_INPUT_PULLDOWN);
    rt_pin_attach_irq(GET_PIN(A, 0), PIN_IRQ_MODE_RISING, BMI088_ERASE_Callback, RT_NULL);
    rt_pin_irq_enable(GET_PIN(A,0),ENABLE);

    return ret;
}
INIT_APP_EXPORT(robotcmd_thread_entry_init);



